import time

import serial
import serial.tools.list_ports as list_ports
from PySide2.QtCore import QTimer
from PySide2.QtUiTools import QUiLoader
from PySide2.QtWidgets import QApplication
from can_common import *

class MainWindow:
    def __init__(self):

        self.ui = QUiLoader().load('./ui/servo_test.ui')
        self.main()
        self.flag = 0
        self.test_timer=QTimer()

    def sing(self):
        self.ui.pushButton_2.clicked.connect(self.get_serial)
        self.ui.pushButton.clicked.connect(self.connect_serial)
        self.ui.pushButton_3.clicked.connect(self.enable_servo)
        self.ui.pushButton_4.clicked.connect(self.disable_servo)
        # self.ui.pushButton_4.clicked.connect(self.send_cmd)
        self.ui.pushButton_6.clicked.connect(self.test)

    # def send_cmd(self):
    #     if self.lineEdit_3!='' and self.lineEdit_6!='':
    #         pass
    #     else:
    #         print('对位置进行控制时，kd 不能赋 0，否则会造成电机震荡，甚至失控。')
    #         self.ui.statusbar.showMessage('对位置进行控制时，kd 不能赋 0，否则会造成电机震荡，甚至失控。', 5000)


    def enable_servo(self):
        if self.flag:
            MotorControl_Start(int(self.ui.lineEdit_2.text()))
            data=self.ser.read(16)
            print(data)

        else:
            self.ui.statusbar.showMessage(f" please open the serial ", 500)
    def disable_servo(self):
        MotorControl_Stop(int(self.ui.lineEdit_2.text()))

    def get_serial(self):
        # 获取可用串口列表
        self.ui.pushButton.setText('打开串口')
        self.ui.comboBox.clear()
        available_ports = list(list_ports.comports())
        for port in available_ports:
            print(port.device)

        # 将可用串口添加到下拉框中
        if len(available_ports) == 0:

            self.ui.comboBox.addItem('no serial ports find ')
        else:

            for port in available_ports:
                self.ui.comboBox.addItem(port.device)
    def connect_serial(self):
        # print(self.ui.comboBox.currentText())
        port_name = self.ui.comboBox.currentText()
        if port_name != 'no serial ports find ':
            try:

                self.ser = serial.Serial(port_name, int(self.ui.lineEdit.text()), timeout=0.5)
                time.sleep(0.5)

                if self.ser.is_open:
                    self.ui.pushButton.setText('Connected')
                    print(f'{port_name} connected')
                    self.flag = 1

                else:
                    self.ui.pushButton.setText('Disconnect')
            except Exception as e:
                if e.errno == 13:

                    self.ui.statusbar.showMessage(f"[Errno 13] Permission denied {port_name}", 500)
                    # self.ui.lineEdit_6.setText()
                print(e)

    def float_to_uint(self,x, x_min, x_max, bits):
        span = x_max - x_min
        offset = x_min
        return np.uint16((x - offset) * ((1 << bits) - 1) / span)
    def test(self):
        # print(self.ui.pushButton_6.text)
        if self.ui.pushButton_6.text()=='start':
            if self.ui.lineEdit_7.text()!='':
                self.f_t=float(self.ui.lineEdit_7.text())
                self.MIT_control('start',self.f_t)
            else:
                self.f_t=0
                self.MIT_control('start', 0)
            self.ui.pushButton_6.setText('stop')
        else:
            self.MIT_control('stop')
            self.ui.pushButton_6.setText('start')

    def MIT_cmd(self):
        self.f_t = -self.f_t
        if self.MIT_flag!=0:
            print(f' self.cmd_timer.isActive(): {self.cmd_timer.isActive()}')
            # self.cmd_timer.stop()
        else:
            self.cmd_timer.start(400)
            self.cmd_timer.timeout.connect(self.send_MIT_cmd)
            self.MIT_flag=1

    def send_MIT_cmd(self):
        print(f'time out {self.f_t} ')
        CanComm_SendControlPara(0, 0, 0, 0, 0, int(self.ui.lineEdit_2.text()))
        # CanComm_SendControlPara(0, 0, 0, 0, self.f_t/2, int(self.ui.lineEdit_2.text()))
        CanComm_SendControlPara(0, 0, 0, 0, self.f_t, int(self.ui.lineEdit_2.text()))
        # time.sleep()
        #
    def MIT_control(self,stats,f_t_1=0):
        def CanComm_SendControlPara(f_p, f_v, f_kp, f_kd, f_t, GM8115_ID):
            # global sendControl_flag, pre_time, s_flag, CAN1_buf1
            send_data = np.array(
                [0x55, 0xAA, 0x1e, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0, 0, 0, 0, 0x00, 0x08,
                 0x00, 0x00, 0, 0, 0, 0, 0, 0, 0, 0, 0x88], np.uint8)

            LIMIT_MIN_MAX(f_p, P_MIN, P_MAX)
            LIMIT_MIN_MAX(f_v, V_MIN, V_MAX)
            LIMIT_MIN_MAX(f_kp, KP_MIN, KP_MAX)
            LIMIT_MIN_MAX(f_kd, KD_MIN, KD_MAX)
            LIMIT_MIN_MAX(f_t, T_MIN, T_MAX)
            p = self.float_to_uint(f_p, P_MIN, P_MAX, 16)
            v = self.float_to_uint(f_v, V_MIN, V_MAX, 12)
            kp = self.float_to_uint(f_kp, KP_MIN, KP_MAX, 12)
            kd = self.float_to_uint(f_kd, KD_MIN, KD_MAX, 12)
            t = self.float_to_uint(f_t, T_MIN, T_MAX, 12)
            buf[0] = GM8115_ID
            buf[1] = p >> 8
            buf[2] = p & 0xFF
            buf[3] = v >> 4
            buf[4] = ((v & 0xF) << 4) | (kp >> 8)
            buf[5] = kp & 0xFF
            buf[6] = kd >> 4
            buf[7] = ((kd & 0xF) << 4) | (t >> 8)
            buf[8] = t & 0xff
            send_data[13] = GM8115_ID
            send_data[21:29] = buf[1:9, 0]
            print(send_data)

            self.ser.write(bytes(send_data.T))
        if stats=='start':
            if self.ui.lineEdit_8!='':
            #
                self.test_timer.start(int(self.ui.lineEdit_8.text())*1000)
            #
                self.test_timer.timeout.connect(self.MIT_cmd)
                self.cmd_timer=QTimer()
                self.MIT_flag=0

                #
        else:

        # time.sleep(0.2)
            CanComm_SendControlPara(0,0,0,0,0,int(self.ui.lineEdit_2.text()))
            self.test_timer.stop()
            self.cmd_timer.stop()

    def main(self):
        self.sing()
        self.get_serial()


if __name__ == "__main__":
    app = QApplication([])
    mainwindow = MainWindow()
    mainwindow.ui.show()
    app.exec_()
